/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
/**
 * 
 */
package robocup.component.worldmodel.particlefilter;

import java.util.Random;

import robocup.component.geometry.Vektor;
import robocup.component.infotypes.BallInfo;

/**
 * @author werwurm
 * 
 */
public class BallSpeedRegion {

	private static double PI_180 = Math.PI / 180.0;

	private static double _180_PI = 180.0 / Math.PI;

	private double minDeltaR;

	private double maxDeltaR;

	private double minDeltaPhi;

	private double maxDeltaPhi;

	private double seenDistance;

	private double ex, ey;

	private double headangle;

	private Vektor mySpeed;

	private Vektor helperVektor = new Vektor();

	private Random random = new Random(System.currentTimeMillis());

	/**
	 * A region, in which the ball-speed may be defined.
	 * 
	 */
	public BallSpeedRegion() {

	}

	public void reinitRegion(BallInfo bi, double headAngle,
			BallRegion posregion, Vektor mySpeed) {
		this.headangle = headAngle;
		this.mySpeed = mySpeed;
		this.minDeltaR = posregion.minDistance
				* (Math.rint(bi.getDistChange() / (bi.getDistance() * .02)) - .5)
				* .02;
		this.maxDeltaR = posregion.maxDistance
				* (Math.rint(bi.getDistChange() / (bi.getDistance() * .02)) + .5)
				* .02;

		this.minDeltaPhi = (Math.rint(bi.getDirChange() / 0.1) - 0.05) * 0.1;
		this.maxDeltaPhi = (Math.rint(bi.getDirChange() / 0.1) + 0.05) * 0.1;

		this.seenDistance = bi.getDistance();

		this.ex = Math.sin(bi.getDirection() * PI_180);
		this.ey = Math.cos(bi.getDirection() * PI_180);
	}

	public Vektor makeRandomInsideRegion(Vektor point) {
		if (point == null)
			point = new Vektor();

		double randDeltaPhi = this.minDeltaPhi + this.random.nextDouble()
				* (this.maxDeltaPhi - this.minDeltaPhi);
		double randDeltaR = this.minDeltaR + this.random.nextDouble()
				* (this.maxDeltaR - this.minDeltaR);

		point.x = randDeltaR * this.ex + randDeltaPhi * PI_180
				* this.seenDistance * this.ey;
		point.y = randDeltaR * this.ey - randDeltaPhi * PI_180
				* this.seenDistance * this.ex;

		point.rotate(this.headangle);
		point.addToThis(this.mySpeed);
		return point;
	}

	public double insideRegion(Vektor v) {
		this.helperVektor.x = v.x;
		this.helperVektor.y = v.y;
		this.helperVektor.subFromThis(this.mySpeed);
		this.helperVektor.rotate(-this.headangle);
		double DeltaPhi = _180_PI * (v.x * this.ey - v.y * this.ex)
				/ this.seenDistance;
		double DeltaR = v.x * this.ex + v.y * this.ey;

		if (this.minDeltaPhi <= DeltaPhi && DeltaPhi <= this.maxDeltaPhi
				&& this.minDeltaR <= DeltaR && DeltaR <= this.maxDeltaR) {
			return 1.0;
		} else
			return -1;
	}

}
